package loppy.dynamics 
{
  import loppy.collision.Collision;
  import loppy.collision.IBroadphase;
  import loppy.collision.NSquared;
  import loppy.collision.ShapePair;
  import loppy.loppy_internal;
  import loppy.math.Vec2;
  import loppy.utils.InList;
  import loppy.utils.InListIterator;
  
  public class World
  {
    use namespace loppy_internal;
    
    private static var idCounter:uint = 0;
    loppy_internal var id  :uint = 0;
    loppy_internal var type:uint = 0;
    
    loppy_internal var bodies:InList = new InList();
    
    loppy_internal var broadphase:IBroadphase = null;
    
    loppy_internal var contactManager:ContactManager = new ContactManager();
    loppy_internal var pairs         :InList = new InList();
    
    loppy_internal var debugDraw:DebugDraw = null;
    public function getDebugDraw():DebugDraw { return debugDraw; }
    public function setDebugDraw(value:DebugDraw):World
    {
      debugDraw = value;
      return this;
    }
    
    loppy_internal var stats:Stats = new Stats();
    public function getStats():Stats { return stats; }
    
    public function World(broadphase:IBroadphase = null)
    {
      if (!broadphase) broadphase = new NSquared();
      this.broadphase = broadphase;
    }
    
    public function add(body:Body):World
    {
      if (body.parent) throw new Error(body + " already added to " + body.parent);
      
      bodies.add(body);
      body.parent = this;
      
      //add shape proxies to broadphase
      for (var shapeIter:InListIterator = body.shapes.begin(); !shapeIter.isEnd(); shapeIter.next())
      {
        var shape:Shape = shapeIter.data();
        broadphase.add(shape.proxy);
      }
      
      return this;
    }
    
    public function remove(body:Body):World
    {
      if (body.parent != this) throw new Error(body + " does not belong to " + this);
      
      //remove shape proxies from broadphase
      for (var shapeIter:InListIterator = body.shapes.begin(); !shapeIter.isEnd(); shapeIter.next())
      while (shapeIter.isEnd())
      {
        var shape:Shape = shapeIter.data();
        broadphase.remove(shape.proxy);
      }
      
      bodies.remove(body);
      body.parent = null;
      
      return this;
    }
    
    public function step(dt:Number, numIterations:uint = 10):void
    {
      var pairIter   :InListIterator;
      var pair       :ShapePair;
      var shapeIter  :InListIterator;
      var shape      :Shape;
      var bodyIter   :InListIterator;
      var body       :Body;
      var contactIter:InListIterator;
      var contact    :Contact;
      
      //update shapes and mass
      for (bodyIter = bodies.begin(); !bodyIter.isEnd(); bodyIter.next())
      {
        body = bodyIter.data();
        body.updateShapeTransforms();
        if (body.massDirty) body.updateMass();
      }
      
      //recycle shape pairs
      for (pairIter = pairs.begin(); !pairIter.isEnd(); pairIter.remove())
      {
        ShapePair.pool.put(pairIter.data());
      }
      
      //broadphase
      broadphase.calculatePairs(pairs);
      
      //evaluate existing contacts
      contactManager.update();
      
      //narrowphase
      for (pairIter = pairs.begin(); !pairIter.isEnd(); pairIter.next())
      {
        pair = pairIter.data();
        
        //detect collision
        var s1:Shape = pair.shape1;
        var s2:Shape = pair.shape2;
        contact = Contact.pool.get();
        contact.shape1 = s1;
        contact.shape2 = s2;
        
        //TODO: optimization - avoid duplicate evaluation from ContactManager.update()
        switch (s1.type | s2.type)
        {
          case Collision.CIRCLE_CIRCLE:
          {
            Collision.collideCircles(contact.contactPoints, CircleShape(s1), CircleShape(s2));
            break;
          }
          case Collision.POLYGON_CIRCLE:
          {
            //s1 must be the box
            if (s2.type == Shape.POLYGON)
            {
              contact.shape1 = s2;
              contact.shape2 = s1;
              s1 = contact.shape1;
              s2 = contact.shape2;
            }
            Collision.collidePolyconCircle(contact.contactPoints, PolygonShape(s1), CircleShape(s2));
            break;
          }
        }
        
        //record contact if colliding
        if (contact.contactPoints.length)
        {
          contactManager.add(contact);
        }
        else
        {
          Contact.pool.put(contact.dispose());
        }
      }
      
      //solve constraints
      //TODO
      
      //integrate positions
      for (bodyIter = bodies.begin(); !bodyIter.isEnd(); bodyIter.next())
      {
        body = bodyIter.data();
        body.integrate(dt);
      }
      
      if (debugDraw)
      {
        debugDraw.clear();
        
        //draw shapes
        if (debugDraw.flags & DebugDraw.FLAG_SHAPE)
        {
          //loop through all bodies
          for (bodyIter = bodies.begin(); !bodyIter.isEnd(); bodyIter.next())
          {
            body = bodyIter.data();
            
            //loop through all shapes
            shapeIter = body.shapes.begin();
            for (shapeIter = body.shapes.begin(); !shapeIter.isEnd(); shapeIter.next())
            {
              shape = shapeIter.data();
              
              //draw shape
              switch (shape.type)
              {
                case Shape.CIRCLE:
                {
                  var circle:CircleShape = CircleShape(shape);
                  debugDraw.drawCircle(circle.globalPosition.x, circle.globalPosition.y, circle.radius, DebugDraw.COLOR_LINE);
                  var pointOnRim  :Vec2 = Vec2.pool.get();
                  var radiusOffset:Vec2 = Vec2.pool.get();
                  pointOnRim.copy(circle.globalPosition).addThis(radiusOffset.set(circle.radius, 0.0).rotateThis(circle.globalRotation));
                  debugDraw.drawLine(circle.globalPosition.x, circle.globalPosition.y, pointOnRim.x, pointOnRim.y, DebugDraw.COLOR_LINE);
                  Vec2.pool.put(pointOnRim);
                  Vec2.pool.put(radiusOffset);
                  break;
                }
                case Shape.POLYGON:
                {
                  var polygon:PolygonShape = PolygonShape(shape);
                  for (var p:int, len:int = polygon.globalVertices.length; p < len; ++p)
                  {
                    var vert1:Vec2 = polygon.globalVertices[ p           ];
                    var vert2:Vec2 = polygon.globalVertices[(p + 1) % len];
                    debugDraw.drawLine(vert1.x, vert1.y, vert2.x, vert2.y, DebugDraw.COLOR_LINE);
                  }
                  break;
                }
              } //END: draw shape
            } //END: loop through all shapes
          } //END:loop through all bodies
        } //END: draw shapes
        
        //draw centers of mass
        if (debugDraw.flags & DebugDraw.FLAG_CENTER_OF_MASS)
        {
          //TODO
        } //END: draw centers of mass
        
        //draw AABBs
        if (debugDraw.flags & DebugDraw.FLAG_AABB)
        {
          for (bodyIter = bodies.begin(); !bodyIter.isEnd(); bodyIter.next())
          {
            for (shapeIter = Body(bodyIter.data()).shapes.begin(); !shapeIter.isEnd(); shapeIter.next())
            {
              var proxy:Proxy = Shape(shapeIter.data()).proxy;
              debugDraw.drawLine(proxy.minX, proxy.minY, proxy.maxX, proxy.minY, DebugDraw.COLOR_AABB);
              debugDraw.drawLine(proxy.maxX, proxy.minY, proxy.maxX, proxy.maxY, DebugDraw.COLOR_AABB);
              debugDraw.drawLine(proxy.maxX, proxy.maxY, proxy.minX, proxy.maxY, DebugDraw.COLOR_AABB);
              debugDraw.drawLine(proxy.minX, proxy.maxY, proxy.minX, proxy.minY, DebugDraw.COLOR_AABB);
            }
          }
        }
        
        //draw contact points
        if (debugDraw.flags & DebugDraw.FLAG_CONTACT)
        {
          //loop through all contacts
          for (contactIter = contactManager.contacts.begin(); !contactIter.isEnd(); contactIter.next())
          {
            contact = contactIter.data();
            
            if (!contact.isTouching) continue;
            
            //loop through all contact points of this contact
            for each (var contactPoint:ContactPoint in contact.contactPoints)
            {
              debugDraw.drawCircle
              (
                contactPoint.position.x, contactPoint.position.y, 
                DebugDraw.CONTACT_RADIUS, DebugDraw.COLOR_CONTACT
              );
              
              //TODO
              debugDraw.drawLine
              (
                contactPoint.position.x + contactPoint.normal.x * 0.5 * DebugDraw.CONTACT_NORMAL, 
                contactPoint.position.y + contactPoint.normal.y * 0.5 * DebugDraw.CONTACT_NORMAL, 
                contactPoint.position.x - contactPoint.normal.x * 0.5 * DebugDraw.CONTACT_NORMAL, 
                contactPoint.position.y - contactPoint.normal.y * 0.5 * DebugDraw.CONTACT_NORMAL, 
                DebugDraw.COLOR_CONTACT
              );
            } //END: loop through all contact points of this contact
          } //END: loop through all contacts
        } //END: draw contact points
      }
    }
    
    public function clear():World
    {
      for (var bodyIter:InListIterator = bodies.begin(); !bodyIter.isEnd(); bodyIter.next())
      {
        var body:Body = bodyIter.data();
        for (var shapeIter:InListIterator = body.shapes.begin(); !shapeIter.isEnd(); shapeIter.next())
        {
          var shape:Shape = shapeIter.data();
          broadphase.remove(shape.proxy);
        }
        body.parent = null;
      }
      
      bodies.clear();
      return this;
    }
    
    public function toString():String
    {
      return "[World id=" + id + "]";
    }
  }
}
